/*****************************************************
** Author:
** Date: 2019-07-25
*
*****************************************************/
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <bcm2835.h>
#include "spi_if.h"
#include "i2c_if.h"
#include "ads1256.h"
#include "ads1256_reg.h"
#include "pca9685.h"


#define log_dbg(_fmt_, ...)   \
            printf("[ADS1256][%s:%d][%s][DBG] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_inf(_fmt_, ...)   \
            printf("[ADS1256][%s:%d][%s][INF] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_wrn(_fmt_, ...)   \
            printf("[ADS1256][%s:%d][%s][WRN] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)
#define log_err(_fmt_, ...)   \
            printf("[ADS1256][%s:%d][%s][ERR] "_fmt_"\r\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__)


#define ADS1256_PIN_CS      22
#define ADS1256_PIN_DRDY    17
#define ADS1256_PIN_RST     18
#define ADS1256_PIN_PDWN    27

#define ADS1256_TEST_NUM    4

#define PCA9685_I2C_ADDR    0x40
#define PCA9685_I2C_RATE    100000
#define PCA9685_PWM_FREQ    50

#define SERVO_CHANNEL_LEFTRIGHT     0
#define SERVO_CHANNEL_UPDOWN        1

#define SERVO_PWM_BASE      0x100

#define SERVO_PWM_LR_MAXIMUM    576
#define SERVO_PWM_LR_MINIMUM    72
#define SERVO_PWM_LR_DEFAULT    344

#define SERVO_PWM_UD_MAXIMUM    464
#define SERVO_PWM_UD_MINIMUM    256
#define SERVO_PWM_UD_DEFAULT    444

enum
{
    JOYSTICK_NONE,
    JOYSTICK_UP,
    JOYSTICK_DOWN,
    JOYSTICK_LEFT,
    JOYSTICK_RIGHT,
};


/**
 * @brief
 * @param  None
 * @retval None
 */
static int ads1256_gpio_init(void)
{
    bcm2835_gpio_fsel(ADS1256_PIN_CS, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(ADS1256_PIN_CS, HIGH);

    bcm2835_gpio_fsel(ADS1256_PIN_DRDY, BCM2835_GPIO_FSEL_INPT);
    bcm2835_gpio_set_pud(ADS1256_PIN_DRDY, BCM2835_GPIO_PUD_UP);

    bcm2835_gpio_fsel(ADS1256_PIN_RST, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(ADS1256_PIN_RST, HIGH);

    bcm2835_gpio_fsel(ADS1256_PIN_PDWN, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_write(ADS1256_PIN_PDWN, HIGH);
    return 0;
}

static unsigned char ads1256_spi_transfer(unsigned char val)
{
    return spi_transfer(val);
}

static void ads1256_spi_csn(unsigned char level)
{
    bcm2835_gpio_write(ADS1256_PIN_CS, level);
    // log_dbg("set cs %d", level);
}

static int ads1256_drdy_input(void)
{
    return bcm2835_gpio_lev(ADS1256_PIN_DRDY);
}

static void ads1256_rst(unsigned char level)
{
    bcm2835_gpio_write(ADS1256_PIN_RST, level);
}

static void ads1256_pdwn(unsigned char level)
{
    bcm2835_gpio_write(ADS1256_PIN_PDWN, level);
}

static void plat_delay_ms(unsigned int ms)
{
    bcm2835_delay(ms);
}

static void plat_delay_us(unsigned int us)
{
    bcm2835_delayMicroseconds(us);
}

static int pca9685_i2c_read(unsigned char *pbuf, int len)
{
    return i2c_read(pbuf, len);
}

static int pca9685_i2c_write(unsigned char *pbuf, int len)
{
    return i2c_write(pbuf, len);
}

static void print_dump_hex(char *str, unsigned char *data, unsigned int len)
{
    printf("%s dump hex %d len:", str, len);
    for (int i=0; i<len; i++)
    {
        if (i % 16 == 0)
            printf("\n%p:", &data[i]);

        printf(" %02x", data[i]);
    }
    printf("\n");
}

static int ads1256_dev_init(void)
{
    ads1256_obj_t obj;

    if (ads1256_gpio_init() != 0)
    {
        log_err("gpio init failed");
        return -1;
    }

    if (spi_init(BCM2835_SPI_BIT_ORDER_MSBFIRST,
                 BCM2835_SPI_MODE1,
                 BCM2835_SPI_CLOCK_DIVIDER_256) != 0)
    {
        log_err("spi init failed");
        return -1;
    }

    obj.spi_transfer_byte = ads1256_spi_transfer;
    obj.spi_csn_level = ads1256_spi_csn;
    obj.reset_level = ads1256_rst;
    obj.sync_pdwn_level = ads1256_pdwn;
    obj.drdy_level = ads1256_drdy_input;
    obj.delay_ms = plat_delay_ms;
    obj.delay_us = plat_delay_us;
    ads1256_if_init(&obj);
    return 0;
}

static int pca9685_dev_init(void)
{
    pca9685_obj_t obj;

    i2c_init(PCA9685_I2C_ADDR, PCA9685_I2C_RATE, BCM2835_I2C_CLOCK_DIVIDER_626);

    obj.i2c_read = pca9685_i2c_read;
    obj.i2c_write = pca9685_i2c_write;
    obj.delay_ms = plat_delay_ms;
    obj.delay_us = plat_delay_us;
    pca9685_if_init(&obj);

    pca9685_init(PCA9685_PWM_FREQ);
    return 0;
}

static void exit_handler(int signo)
{
    //System Exit
    log_inf("Program Stop");

    pca9685_deinit();

    i2c_deinit();
    spi_deinit();
    bcm2835_close();

    exit(0);
}


static void joystick_get_direction(float ud_volt, float lr_volt, int *ud, int *lr)
{
#define HIGH_THRESHOLD  3.0
#define LOW_THRESHOLD   0.5

    if (ud_volt < LOW_THRESHOLD)
        *ud =  JOYSTICK_UP;
    else if (ud_volt > HIGH_THRESHOLD)
        *ud = JOYSTICK_DOWN;
    else
        *ud = JOYSTICK_NONE;

    if (lr_volt < LOW_THRESHOLD)
        *lr = JOYSTICK_RIGHT;
    else if (lr_volt > HIGH_THRESHOLD)
        *lr = JOYSTICK_LEFT;
    else
        *lr = JOYSTICK_NONE;
}

int main(int argc, char const *argv[])
{
    unsigned char val;
    int i, j, cnt, chan[ADS1256_TEST_NUM], data[ADS1256_TEST_NUM]; // , difftime[ADS1256_TEST_NUM];
    float volt[ADS1256_TEST_NUM];
    int ud, lr;
    unsigned short servo_lr_pwm = SERVO_PWM_BASE + SERVO_PWM_LR_DEFAULT;
    unsigned short servo_ud_pwm = SERVO_PWM_BASE + SERVO_PWM_UD_DEFAULT;

    if (!bcm2835_init())
    {
        log_err("bcm2835_init failed. Are you running as root??\n");
        return -1;
    }

    if (ads1256_dev_init() < 0)
    {
        log_err("ads1256 init failed");
        return -1;
    }

    if (pca9685_dev_init() < 0)
    {
        log_err("pca9685 init failed");
        return -1;
    }

    // Exception handling:ctrl + c
    signal(SIGINT, exit_handler);

    log_inf("init done");

    val = ads1256_read_status();
    log_dbg("read status 0x%x", val);

    val = ads1256_chipid();
    log_dbg("chip id 0x%x", val);

    ads1256_cfg(0x1, 0x0, ADS1256_GAIN_1, ADS1256_DRATE_3750SPS);

    log_dbg("==== ads1256 single ended ====");

    cnt = 1000;
    for (i = 0; i < ADS1256_TEST_NUM; i++)
        chan[i] = i;

    // drop the first data
    ads1256_read_data();

    // set servo to default position
    pca9685_set_pwm(SERVO_CHANNEL_UPDOWN, SERVO_PWM_BASE, servo_ud_pwm);
    pca9685_set_pwm(SERVO_CHANNEL_LEFTRIGHT, SERVO_PWM_BASE, servo_lr_pwm);

    // while (cnt--)
    while (1)
    {
        log_dbg("==== cnt %d ====", cnt);

        ads1256_scan_single_ended_channels(chan, ADS1256_TEST_NUM, data);
        for (i = 0; i < ADS1256_TEST_NUM; i++)
        {
            volt[i] = data[i] * 5.0 / 0x7fffff;
            log_dbg("chan[%d] data 0x%06x volt %fV", i, data[i], volt[i]);
        }

        joystick_get_direction(volt[3], volt[2], &ud, &lr);

        switch (ud)
        {
            case JOYSTICK_DOWN:
                servo_ud_pwm += 1;
                if (servo_ud_pwm > (SERVO_PWM_BASE + SERVO_PWM_UD_MAXIMUM))
                    servo_ud_pwm = (SERVO_PWM_BASE + SERVO_PWM_UD_MAXIMUM);

                pca9685_set_pwm(SERVO_CHANNEL_UPDOWN, SERVO_PWM_BASE, servo_ud_pwm);
                break;
            case JOYSTICK_UP:
                servo_ud_pwm -= 1;
                if (servo_ud_pwm < (SERVO_PWM_BASE + SERVO_PWM_UD_MINIMUM))
                    servo_ud_pwm = (SERVO_PWM_BASE + SERVO_PWM_UD_MINIMUM);

                pca9685_set_pwm(SERVO_CHANNEL_UPDOWN, SERVO_PWM_BASE, servo_ud_pwm);
                break;
        }
        log_dbg("ud %d servo_ud_pwm %d", ud, servo_ud_pwm);

        switch (lr)
        {
            case JOYSTICK_LEFT:
                servo_lr_pwm += 1;
                if (servo_lr_pwm > (SERVO_PWM_BASE + SERVO_PWM_LR_MAXIMUM))
                    servo_lr_pwm = (SERVO_PWM_BASE + SERVO_PWM_LR_MAXIMUM);

                pca9685_set_pwm(SERVO_CHANNEL_LEFTRIGHT, SERVO_PWM_BASE, servo_lr_pwm);
                break;
            case JOYSTICK_RIGHT:
                servo_lr_pwm -= 1;
                if (servo_lr_pwm < (SERVO_PWM_BASE + SERVO_PWM_LR_MINIMUM))
                    servo_lr_pwm = (SERVO_PWM_BASE + SERVO_PWM_LR_MINIMUM);

                pca9685_set_pwm(SERVO_CHANNEL_LEFTRIGHT, SERVO_PWM_BASE, servo_lr_pwm);
                break;
        }
        log_dbg("lr %d servo_lr_pwm %d", lr, servo_lr_pwm);

        if (cnt)
        {
            /*
             *  cnt show 1 line
             *  adc channels lines
             *  joystick 2 lines
             */
            printf("\33[%dA", 1 + ADS1256_TEST_NUM + 2);
        }
    }

    // log_dbg("==== read continuous ====");
    // for (i = 0; i < 3; i++)
    // {
    //     ads1256_read_single_ended_channel_continuous(i, ADS1256_TEST_CONTINUOUS_NUM, data, difftime);

    //     /*
    //      *  It always reads a value from previous conversion!
    //      */
    //     for (j = 1; j < ADS1256_TEST_CONTINUOUS_NUM; j++)
    //     {
    //         volt = data[j] * 5.0 / 0x7fffff;
    //         log_dbg("chan[%d] sample[%d] data 0x%x volt %fV difftime %d",
    //                                 i, j, data[j], volt, difftime[j]);
    //     }

    //     bcm2835_delay(1 * 1000);
    // }

    exit_handler(0);
    return 0;
}

/* End of file */



